%%
clear;clc
syms s zeta k w0 deepth width f fs1 fs2 z 
syms Ts deepth f_width f0 w_width w

assume(deepth, 'positive')
assume(w_width, 'positive')
assume(w0, 'positive')
assume(w, 'positive')
%% init param
% s=tf('s')
% Ts = double(1e-4)
% deepth = double(0.5)
% f_width = double(20)
% f0 = double(100)

%% get system tf
% z = tf('z', Ts)
% s = (1-z^-1)/Ts
% s = 2/Ts*(z-1)/(z+1);%tustin

% equ = [deepth == zeta, ...
%         f_width == 2*k*f0, ...
%         f0 == w0/2/pi]
% 
% [zeta k w0] = solve(equ, [zeta k w0])

% zeta = single(zeta);
% k = single(k);
% w0 = single(w0);
% w0 = 2*pi*f0
% w_width = 2*pi*f_width




fs1 = s^2 + w_width*deepth*s + w0^2;
fs2 = s^2 + w_width*s + w0^2;
% fs1 = s^2 + w0^2;
% fs2 = s^2 + 2*w_width*s + w0^2;

fs = fs1/fs2;

%%
fs_w = subs(fs, s, j*w)
equ = abs(fs_w)==deepth
sol = solve(equ,w)
width_real = simplify(sol(1)-sol(2))
single(subs(width_real,[w_width,deepth,w0],[2*pi*20, 0.5, 2*pi*100]))
%% simplify
fs_tmp = fs
fs_tmp = simplify(fs_tmp)
fs_tmp = collect(fs_tmp, z)
[num, den] = numden(fs_tmp);
fs_num_coeff = coeffs(num, z);
fs_den_coeff = coeffs(den, z);
k = fs_den_coeff(end)
m = flip(fs_num_coeff/k)
n = flip(fs_den_coeff(1:end-1)/k)
m = single(m)
n = single(n)

%% s bode plot
figure(1)
bode(1/fs1)
hold on
bode(1/fs2)

figure(2)
bode(fs_tmp)

%% z bode plot
dbode(single(fs_num_coeff), single(fs_den_coeff), Ts)

%% test sin
% Ts = 1e-4
% 
% f = 5
% t = [0:1:5000].' * 1e-4
% sin_data = sin(2*pi*f*t)
% plot(t, sin_data)


%% function
% function y = notch_filter(x, x_last1, x_last2, y_last1, y_last2, m, n):
%     y = (m[0]*x + m[1]*x_last1 + m[2]*x_last2) - ...
%              (n[0]*y_last1 + n[1]*y_last2)
% end